#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

static int _non_equal_points(const cv::Mat& m1, const cv::Mat& m2) {
    int cn = m1.channels();
    CV_Assert(cn == 1);
    CV_Assert(m2.channels() == cn);
    CV_Assert(m1.type() == CV_64FC1);
    CV_Assert(m1.type() == m2.type());
    CV_Assert(m1.rows == m2.rows && m1.cols == m2.cols);
    
    std::vector<cv::Point> non_equal_points;
    for (int i = 0; i < m1.rows; i++) {
        for (int j = 0; j < m1.cols; j++) {
            double d1 = m1.ptr<double>(i)[j];
            double d2 = m2.ptr<double>(i)[j];
            if (std::abs(d1-d2) > FLT_EPSILON) {
                non_equal_points.push_back(cv::Point(j, i));
            }
        }
    }
    return non_equal_points.size();
}

static void _integral(const cv::Mat& src, cv::Mat& sum, cv::Mat& sqsum) {
    CV_Assert(src.channels() == 1);
    CV_Assert(src.type() == CV_64FC1);
    int rows = src.rows;
    int cols = src.cols;
    sum.create(rows+1, cols+1, src.type());
    sqsum.create(rows+1, cols+1, src.type());
    
    for (int X = 0; X < cols+1; X++) {
        sum.ptr<double>(0)[X] = 0;
        sqsum.ptr<double>(0)[X] = 0;
    }
    
    for (int Y = 0; Y < rows; Y++) {
        sum.ptr<double>(Y+1)[0] = 0;
        sqsum.ptr<double>(Y+1)[0] = 0;
        
        double s = 0;
        double sq = 0;
        for (int X = 0; X < cols; X++) {
            double it = src.ptr<double>(Y)[X];
            s += it;
            sq += (double)it*it;
            sum.ptr<double>(Y+1)[X+1] = sum.ptr<double>(Y)[X+1] + s;
            sqsum.ptr<double>(Y+1)[X+1] = sqsum.ptr<double>(Y)[X+1] + sq;
        }
    }
}

static void _integral_tilted(const cv::Mat& src, cv::Mat& tilted) {
    CV_Assert(src.channels() == 1);
    CV_Assert(src.type() == CV_64FC1);
    int rows = src.rows;
    int cols = src.cols;
    tilted.create(rows+1, cols+1, src.type());
    
    for (int X = 0; X < cols+1; X++) {
        tilted.ptr<double>(0)[X] = 0;
    }
    
    tilted.ptr<double>(1)[0] = 0;
    std::vector<double> buf(cols+1);
    for (int X = 0; X < cols; X++) {
        double it = src.ptr<double>(0)[X];
        buf[X] = it;
        tilted.ptr<double>(1)[X+1] = it;
    }
    if (cols == 1) {
        buf[1] = 0;
    }
    
    for (int Y = 1; Y < rows; Y++) {
        double it = src.ptr<double>(Y)[0];
        double t0 = it;
        tilted.ptr<double>(Y+1)[0] = tilted.ptr<double>(Y)[1];
        tilted.ptr<double>(Y+1)[1] = tilted.ptr<double>(Y)[1] + t0 + buf[1];
        
        for (int X = 1; X < cols-1; X++) {
            double t1 = buf[X];
            buf[X-1] = t1 + t0;
            t0 = it = src.ptr<double>(Y)[X];
            t1 += buf[X+1] + t0 + tilted.ptr<double>(Y)[X];
            tilted.ptr<double>(Y+1)[X+1] = t1;
        }
        if (cols > 1) {
            double t1 = buf[cols-1];
            buf[cols-2] = t1 + t0;
            t0 = it = src.ptr<double>(Y)[cols-1];
            tilted.ptr<double>(Y+1)[cols] = t0 + t1 + tilted.ptr<double>(Y)[cols-1];
            buf[cols-1] = t0;
        }
    }
}

int main(int argc, char **argv) {
    cv::Mat src(1000, 1000, CV_64FC1);
    cv::randu(src, cv::Scalar(-1000), cv::Scalar(1000));
    
    cv::Mat sum1, sum2, sqsum1, sqsum2, tilted1, tilted2;
    //cv::integral(src, sum1, sqsum1, CV_64FC1, CV_64FC1);
    cv::integral(src, sum1, sqsum1, tilted1, CV_64FC1, CV_64FC1);
    
    _integral(src, sum2, sqsum2);
    _integral_tilted(src, tilted2);
    
    int non_equals = _non_equal_points(sum1, sum2);
    std::cout << "sum not equals points number = " << non_equals << std::endl;
    std::cout << "sum results are " << (non_equals == 0 ? "same" : "not same") << std::endl;
    
    non_equals = _non_equal_points(sqsum1, sqsum2);
    std::cout << "sqsum not equals points number = " << non_equals << std::endl;
    std::cout << "sqsum results are " << (non_equals == 0 ? "same" : "not same") << std::endl;
    
    non_equals = _non_equal_points(tilted1, tilted2);
    std::cout << "tilted not equals points number = " << non_equals << std::endl;
    std::cout << "tilted results are " << (non_equals == 0 ? "same" : "not same") << std::endl;
    
    return 0;
}
